<template>
  <div class="content"><div id="map"></div></div>
</template>
<script>
import ROSLIB from "roslib";
export default {
  data() {
    return {
      ros: null, //ros客户端
      viewer: null, //viewer容器
      gridClient: null, //地图客户端
      pathShape: null, //路径对象
      pathTopic: null, //路径主题
      startX: 0, //路径起始位置x
      startY: 0, //路径起始位置y
      endX: 0, //路径目标位置x
      endY: 0, //路径目标位置y
      isdrawing: false, // 标记是否正在绘制路径
      lineContainer: null, //临时路径
      line: null, //临时路径线段
      pointContainer: null, //临时路径起点对象
      posTopic: null, //机器人实时位置主题
      pos: null, //机器人实时位置
      directionLine: null, //机器人行进方向
      point: null, //临时路径点
      goalPose: null, //目标位置消息
      goalPosePublisher: null, //目标位置发布器
      ratio: 305 / 250, //地图原始宽高比
      // resizeWidth: window.innerWidth - 220, //地图自适应宽度
      // resizeHeight: ((window.innerWidth - 220) / 305) * 250, //地图自适应高度
    };
  },
  mounted() {
    // window.addEventListener("resize", this.handler);
    this.init();
    // this.handler();
    window.addEventListener("resize", this.handleResize);
  },
  beforeDestroy() {
    window.removeEventListener("resize", this.handleResize);
    this.ros.close();
  },
  methods: {
    init() {
      this.ros = new ROSLIB.Ros({
        url: "ws://192.168.40.130:9090",//连接rosbridge的websocket地址
      });
      const ratio = 125 / 116;
      let newWidth = window.innerWidth - 450;
      let newHeight = window.innerHeight - 150;
      if ((window.innerWidth - 450) / (window.innerHeight - 150) > ratio) {
        newWidth = (window.innerHeight - 150) * ratio;
        newHeight = window.innerHeight - 150;
      } else {
        newWidth = window.innerWidth - 450;
        newHeight = (window.innerWidth - 450) / ratio;
      }
      this.viewer = new ROS2D.Viewer({
        divID: "map",
        width: newWidth,
        height: newHeight,
        antialias: true,
        // width:305,
        // height:250
      });
      console.log("Viewer created successfully", this.viewer);
      // 监听连接
      this.ros.on("connection", () => {
        console.log("成功连接到ROS");
      });
      // 监听错误
      this.ros.on("error", (error) => {
        console.log("ROS连接错误: ", error);
      });
      // 监听关闭
      this.ros.on("close", () => {
        console.log("ROS连接关闭");
      });
      // 1、初始化地图客户端
      this.gridClient = new ROS2D.OccupancyGridClient({
        ros: this.ros,
        rootObject: this.viewer.scene,
        continuous: true,
      });
      // Scale the canvas to fit to the map
      this.gridClient.on("change", () => {
        if (this.viewer) {
          this.viewer.scaleToDimensions(
            this.gridClient.currentGrid.width,
            this.gridClient.currentGrid.height
          );
          if (this.gridClient.currentGrid.pose) {
            this.viewer.shift(
              this.gridClient.currentGrid.pose.position.x,
              this.gridClient.currentGrid.pose.position.y
            );
          }
        }
      });

      // 创建一个Shape对象来显示点
      this.pointContainer = new createjs.Container();
      this.viewer.scene.addChild(this.pointContainer);

      //机器人实时位置
      this.posTopic = new ROSLIB.Topic({
        ros: this.ros,
        name: "/odom",
        messageType: "nav_msgs/msg/Odometry",
      });
      this.posTopic.subscribe((message) => {
        // 设置点的位置为线段的起始位置
        if (this.pos != null) this.pointContainer.removeChild(this.pos);
        this.pos = new createjs.Shape();
        this.pos.graphics.beginFill(createjs.Graphics.getRGB(0, 0, 255)); // 点的填充颜色
        this.pos.graphics.drawRect(-0.15, -0.15, 0.25, 0.25); // 绘制矩形
        // this.pos.graphics.drawCircle(0,0,0.02); // 绘制矩形
        // console.log(message.pose.pose.position.x,message.pose.pose.position.y)
        this.pos.x = message.pose.pose.position.x + 1.9909438479287;
        this.pos.y = -(message.pose.pose.position.y + 0.5133748912421);
        let rotation = this.viewer.scene.rosQuaternionToGlobalTheta(
          message.pose.pose.orientation
        );
        this.pos.rotation = rotation;
        // 在矩形的右侧边的中心绘制一条短的线段表示前进方向
        if (this.directionLine != null)
          this.pointContainer.removeChild(this.directionLine);
        this.directionLine = new createjs.Shape();
        let dx = 0.3 * Math.cos(rotation * (Math.PI / 180));
        let dy = 0.3 * Math.sin(rotation * (Math.PI / 180));
        this.directionLine.graphics.setStrokeStyle(0.01).beginStroke("blue"); // 设置线条样式
        this.directionLine.graphics.moveTo(0, 0); // 起点
        this.directionLine.graphics.lineTo(dx, dy); // 终点
        this.directionLine.graphics.endStroke();

        this.directionLine.x = message.pose.pose.position.x + 1.9909438479287;
        this.directionLine.y = -(
          message.pose.pose.position.y + 0.5133748912421
        );
        // 将线段添加到舞台
        this.pointContainer.addChild(this.directionLine);
        //将矩形添加到舞台
        this.pointContainer.addChild(this.pos);
      }); 
 
      // 2、初始化路径对象
      this.pathShape = new ROS2D.PathShape({
        strokeSize: 0.01, // 设置路径线条宽度
        strokeColor: createjs.Graphics.getRGB(0, 0, 255), // 设置路径颜色（蓝色）
      });
      this.viewer.scene.addChild(this.pathShape);

      // 路径数据来自 ROS 话题 '/plan'（路径规划结果）
      this.pathTopic = new ROSLIB.Topic({
        ros: this.ros,
        name: "/plan",
        messageType: "nav_msgs/msg/Path",
      });

      // 当路径数据到达时，更新路径显示
      this.pathTopic.subscribe((message) => {
        // console.log(message);
        this.pathShape.setPath(message);
      });

      // 可选：显示路径（当 ROS 2 中路径话题接收到数据时自动更新）
      this.pathShape.visible = true;

      // 3、绘制在地图上的目标路径
      (this.startX = 0), (this.startY = 0), (this.endX = 0), (this.endY = 0);
      this.isdrawing = false; // 标记是否正在绘制路径

      // 创建一条直线，使用 createjs.Shape
      this.lineContainer = new createjs.Container();
      this.viewer.scene.addChild(this.lineContainer);

      // 监听鼠标点击 获取起始位置并绘制出发点
      this.viewer.scene.addEventListener("stagemousedown", (event) => {
        if (event.nativeEvent.button === 0) {
          this.isdrawing = true;
          // 获取点击位置
          let mousePos = this.viewer.scene.globalToRos(
            event.stageX,
            event.stageY
          );
          // console.log("Mouse start at: ", mousePos);
          this.startX = mousePos.x;
          this.startY = mousePos.y;

          // 绘制起点
          this.line = new createjs.Shape();
          this.lineContainer.addChild(this.line);
          this.line.graphics.clear();
          this.line.graphics
            .setStrokeStyle(2)
            .beginStroke(createjs.Graphics.getRGB(0, 0, 255)); // 线段为蓝色
          this.line.graphics.moveTo(this.startX, this.startY); // 设置起点

          // 绘制起始位置的点（一个圆形）
          this.point = new createjs.Shape();
          this.point.graphics.beginFill(createjs.Graphics.getRGB(0, 0, 255)); // 点的填充颜色
          this.point.graphics.drawCircle(0, 0, 0.03); // 绘制圆形

          // 设置点的位置为线段的起始位置
          this.point.x = this.startX;
          this.point.y = -this.startY;

          // 将点添加到实例viewer中
          this.pointContainer.addChild(this.point);
        }
      });

      // 监听鼠标拖动，更新轨迹为一条直线
      this.viewer.scene.addEventListener("stagemousemove", (event) => {
        if (this.isdrawing) {
          // 是否正在绘制路径
          // 清除之前的线并绘制从起始位置到当前鼠标位置的直线
          let mousePos = this.viewer.scene.globalToRos(
            event.stageX,
            event.stageY
          );
          // console.log('Mouse move to: ', mousePos);
          this.line.graphics.clear();
          this.line.graphics
            .setStrokeStyle(0.01)
            .beginStroke(createjs.Graphics.getRGB(0, 0, 255));
          this.line.graphics.moveTo(this.startX, -this.startY); // 起始点
          this.line.graphics.lineTo(mousePos.x, -mousePos.y); // 终点是当前鼠标位置
        }
      });

      // 监听鼠标松开，获取终点  向/goal_pose话题发布临时路径规划始末位置
      this.viewer.scene.addEventListener("stagemouseup", (event) => {
        this.isdrawing = false;
        // 获取线段的终点
        let mousePos = this.viewer.scene.globalToRos(
          event.stageX,
          event.stageY
        );
        // console.log("Mouse end at: ", mousePos);
        this.endX = mousePos.x;
        this.endY = mousePos.y;

        let targetYaw = Math.atan2(
          this.endY - this.startY,
          this.endX - this.startX
        );
        // 创建目标位置消息
        this.goalPose = new ROSLIB.Message({
          header: {
            frame_id: "map",
          },
          pose: {
            position: {
              x: this.startX,
              y: this.startY,
              z: 0,
            },
            orientation: {
              x: 0,
              y: 0,
              z: Math.sin(targetYaw / 2),
              w: Math.cos(targetYaw / 2),
            },
          },
        });

        // 创建目标位置发布器
        this.goalPosePublisher = new ROSLIB.Topic({
          ros: this.ros,
          name: "/goal_pose",
          messageType: "geometry_msgs/msg/PoseStamped",
        });

        // 发布目标位置消息
        this.goalPosePublisher.publish(this.goalPose);

        // 清除当前的线
        this.lineContainer.removeChild(this.line);
        this.pointContainer.removeChild(this.point);
        (this.line = null), (this.point = null);
      });
    },
    handleResize() {
      this.ros.close();
      let can = document.querySelector("canvas");
      can.parentNode.removeChild(can);
      this.init();
    },
  },
};
</script>
<style lang="less" scoped>
.content {
  // height: 100vh;
  display: flex;
  justify-content: center;
  align-items: center;
}
</style>
